//    SCHEDA POTENZA versione 1.2 04/05/2012     //

float VstatusBattery[6]; //0=MASTER  1=POTENZA   2=MOTORI   3=RADIO    4=TELECAMERE    5=SENSORI
int BatteryPercent[6]; //0=MASTER  1=POTENZA   2=MOTORI   3=RADIO    4=TELECAMERE    5=SENSORI
boolean sw, stato[7][2]={{true, true},{true, true},{true, true},{true, true},{true, true},{true, true},}; //0=MASTER  1=POTENZA   2=MOTORI   3=RADIO    4=TELECAMERE    5=SENSORI  6=12V(solo on off) carica o scarica/accesa o spenta
float tensione_master[11],corrente_master[11],tensione_power[11],corrente_power[11],tensione_motori[11],tensione_radio5,tensione_radio3,corrente_motori[11],corrente_radio5,corrente_radio3,tensione_camera5[11],tensione_camera3[11],corrente_camera5[11],corrente_camera3[11],tensione_sensor5[11],tensione_sensor3[11],corrente_sensor5[11],corrente_sensor3[11],corrente_elmo,corrente_10V,tensione_5V,corrente_5V,tensione_12V;
float tensione_min_nodo = 4,corrente_max_nodo = 400,corrente_max_5V = 1,corrente_max_motori = 4,tensione_batteria_min = 10;
float total_power;
float offset_corrente, m_corrente;
const float Aref = 4.84;  // Aref=proveniente dal convertitore switching 12-->5 V
const float ValADC = 1023;
const float soglia_min = 3.3; //in V 3.3
const float soglia_max = 4.23; //in V 4.23
int t = 42;  //t indica il pin sull'arduino per sw1 e sw2   t=42
int j=0; //indice tabella medie (0-9 dati, 10 media)
unsigned long time_count=0,Tc=1*1000,WDT=0;
#include <Wire.h>
#define NUM_DATI 16
int i2c=0;

void setup() {
  //CONFIGURAZIONE PIN PER IL MUX
  // EN--->PIN31
  // A0--->PIN33
  // A1--->PIN35
  // A2--->PIN37
  // A3--->PIN29
  pinMode(31,OUTPUT);
  pinMode(33,OUTPUT);
  pinMode(35,OUTPUT);
  pinMode(37,OUTPUT);
  pinMode(29,OUTPUT);
  ///////////////////////////////
  analogReference(DEFAULT); // Aref=proveniente dal convertitore switching 12-->5 V
  ///////////////////////////////
  //Configurazione pin Arduino per sw1 e sw2
  pinMode(52,OUTPUT);  //pin52 arduino corrisponde a uscita sw1 per la scheda sensori
  pinMode(53,OUTPUT);  //pin53 arduino corrisponde a uscita sw2 per la scheda sensori
  pinMode(50,OUTPUT);  //pin50 arduino corrisponde a uscita sw1 per la scheda telecamere
  pinMode(51,OUTPUT);  //pin51 arduino corrisponde a uscita sw2 per la scheda telecamere
  pinMode(48,OUTPUT);  //pin48 arduino corrisponde a uscita sw1 per la scheda radio
  pinMode(49,OUTPUT);  //pin49 arduino corrisponde a uscita sw2 per la scheda radio
  pinMode(46,OUTPUT);  //pin46 arduino corrisponde a uscita sw1 per la scheda motori
  pinMode(47,OUTPUT);  //pin47 arduino corrisponde a uscita sw2 per la scheda motori
  pinMode(44,OUTPUT);  //pin44 arduino corrisponde a uscita sw1 per la scheda potenza
  pinMode(45,OUTPUT);  //pin45 arduino corrisponde a uscita sw2 per la scheda potenza
  pinMode(42,OUTPUT);  //pin42 arduino corrisponde a uscita sw1 per la scheda master
  pinMode(43,OUTPUT);  //pin43 arduino corrisponde a uscita sw2 per la scheda master
  
  pinMode(40,OUTPUT);  // switch 12V (mettere resistenza RIN per limitare corrente in uscita)
  pinMode(12,OUTPUT);  // interrupt verso main
  
  pinMode(13, OUTPUT); //led notifica 
  pinMode(41, OUTPUT); //PWM 
  /////////////////////////////////
  //inizializzazione degli switch (modo carica)
  digitalWrite(52,HIGH);
  digitalWrite(50,HIGH);
  digitalWrite(48,HIGH);
  digitalWrite(46,HIGH);
  digitalWrite(44,HIGH);
  digitalWrite(42,HIGH);
  digitalWrite(53,LOW);
  digitalWrite(51,LOW);
  digitalWrite(49,LOW);
  digitalWrite(47,LOW);
  digitalWrite(45,LOW);
  digitalWrite(43,LOW);
  ///////////////////////////////////////////////////
  digitalWrite(40,HIGH); //12v attivi
  digitalWrite(12,HIGH); //attivo basso
  //Inizializzazione caratteristica sensore di corrente
  m_corrente=0.001f;
  offset_corrente=2.4613f;
  ///////////////////////////////////////////////////
  //Serial.begin(9600);      //da togliere quando è tutto completo (togliere anche tutti i Serial.print)
  
  Wire.begin(1);
  Wire.onRequest(requestEvent);
  Wire.onReceive(receiveEvent);
  
  pinMode(12, OUTPUT); //interrupt 
  digitalWrite(12,LOW); //notifica reset
  delay(100);
  digitalWrite(12,HIGH);
  digitalWrite(41,HIGH); //FAN
}

void loop() { 
if (millis()-time_count>=Tc)
  {
  digitalWrite(13, HIGH);
  time_count=millis();
  
  int k=0; //contatore medie
  for(j=0;j<10;j++)
    {
    
    //Serial.println("MASTER  POTENZA MOTORI RADIO    CAMERA   SENSORI        V_STATUS BACKPACK");
    
    ///////////////// GESTIONE SWITCH ////////////////////////
    for (int i=0; i<6; i++){
      VstatusBattery[i] = analogRead(i);  //legge V_status della scheda i-esima
      VstatusBattery[i] = (Aref*VstatusBattery[i])/ValADC;
      BatteryPercent[i] = int(((VstatusBattery[i]-soglia_min)/(soglia_max-soglia_min))*100.0f); //calcola il livello carica batterie in percentuale
      //Serial.print(BatteryPercent[i]);
      //Serial.print("%");
      if (VstatusBattery[i] < soglia_min) // If Vstatus < 3.3V BackPack needs to be charge
      {
        if (stato[i][1])
        {
          stato[i][0]=true;
          digitalWrite(t,HIGH);
          delay(10);  //ritardo 10 ms per non creare buchi nell'alimentazione
          t++;
          digitalWrite(t,LOW);
          t++;
          //Serial.print("R");
        }
      }
      else if ((VstatusBattery[i] >= soglia_max )&&(i!=2)) // If Vstatus ok, will work with ARDUINO Backpack
      {
        if (stato[i][1])
        {
          stato[i][0]=false;
          t++;
          digitalWrite(t,HIGH);
          delay(10);                            
          t--;
          digitalWrite(t,LOW);
          t=t+2;
          //Serial.print("S");
        }
      }
      else
      {
        t=t+2;
      }
      //Serial.print("    ");  
    }
    
    t=42;
    ///////////////////////////////////////////////////////////////////////////
    //Serial.println("");
    //////////////////////////////////////////////////////////////////////////////////////////////
    
    //Serial.println("MASTER           POTENZA            MOTORI            RADIO5           RADIO3           CAMERA5         CAMERA3           SENSORI5        SENSORI3       MOTORI   10V      5V              12V      TENSIONI/CORRENTI ");
    
    /////////////////////////MISURA DELLE CORRENTI E DELLE TENSIONI///////////////////////////////////////
    
    
    ///////TENSIONE MASTER S1////// A=0000
    digitalWrite(33,LOW); 
    digitalWrite(35,LOW); 
    digitalWrite(37,LOW); 
    digitalWrite(29,LOW); 
    digitalWrite(31,HIGH);
    delay(1);
    tensione_master[j] = analogRead(15);
    tensione_master[j]=(Aref*tensione_master[j])/ValADC;
    digitalWrite(31,LOW);
    tensione_master[10]=0;
    for(k=0;k<10;k++){
      tensione_master[10]=tensione_master[k]+tensione_master[10];
      }
    //Serial.print(tensione_master[10]/10);
    //Serial.print(" ");
    /////////////////////
    // MEASUREMENT CURRENT ARDUINO MASTER
    corrente_master[j] = analogRead(6);
    corrente_master[j]=(Aref*corrente_master[j])/ValADC; // Digital data become in Voltage value
    corrente_master[j]=-((corrente_master[j]-offset_corrente)/m_corrente); // in mA
    corrente_master[10]=0;
    for(k=0;k<10;k++){
      corrente_master[10]=corrente_master[k]+corrente_master[10];
      }
    //Serial.print(corrente_master[10]/10);
    //Serial.print("      ");
    
    ///////TENSIONE POTENZA S2////// A=0001
    digitalWrite(33,HIGH); 
    digitalWrite(35,LOW); 
    digitalWrite(37,LOW); 
    digitalWrite(29,LOW); 
    digitalWrite(31,HIGH);
    delay(1);
    tensione_power[j] = analogRead(15);
    tensione_power[j]=(Aref*tensione_power[j])/ValADC;
    digitalWrite(31,LOW);
    tensione_power[10]=0;
    for(k=0;k<10;k++){
      tensione_power[10]=tensione_power[k]+tensione_power[10];
      }
    //Serial.print(tensione_power[10]/10);
    //Serial.print(" ");
    /////////////////////
    // MEASUREMENT CURRENT ARDUINO POTENZA
    corrente_power[j] = analogRead(7);
    corrente_power[j]=(Aref*corrente_power[j])/ValADC; // Digital data become in Voltage value
    corrente_power[j]=-((corrente_power[j]-offset_corrente)/m_corrente); // in mA
    corrente_power[10]=0;
    for(k=0;k<10;k++){
      corrente_power[10]=corrente_power[k]+corrente_power[10];
      }
    //Serial.print(corrente_power[10]/10);
    //Serial.print("        ");
    
    ///////TENSIONE MOTORI S3////// A=0010
    digitalWrite(33,LOW); 
    digitalWrite(35,HIGH); 
    digitalWrite(37,LOW); 
    digitalWrite(29,LOW); 
    digitalWrite(31,HIGH);
    delay(1);
    tensione_motori[j] = analogRead(15);
    tensione_motori[j]=(Aref*tensione_motori[j])/ValADC;
    digitalWrite(31,LOW);
    tensione_motori[10]=0;
    for(k=0;k<10;k++){
      tensione_motori[10]=tensione_motori[k]+tensione_motori[10];
      }
    //Serial.print(tensione_motori[10]/10);
    //Serial.print(" ");
    /////////////////////
    // MEASUREMENT CURRENT ARDUINO MOTORI
    corrente_motori[j] = analogRead(8);
    corrente_motori[j]=(Aref*corrente_motori[j])/ValADC; // Digital data become in Voltage value
    corrente_motori[j]=-((corrente_motori[j]-offset_corrente)/m_corrente); // in mA
    corrente_motori[10]=0;
    for(k=0;k<10;k++){
      corrente_motori[10]=corrente_motori[k]+corrente_motori[10];
      }
    //Serial.print(corrente_motori[10]/10);
    //Serial.print("      ");
    
    ///////TENSIONE RADIO 5V S4////// A=0011
    digitalWrite(33,HIGH); 
    digitalWrite(35,HIGH); 
    digitalWrite(37,LOW); 
    digitalWrite(29,LOW); 
    digitalWrite(31,HIGH);
    delay(1);
    tensione_radio5 = analogRead(15);
    tensione_radio5=(Aref*tensione_radio5)/ValADC;
    digitalWrite(31,LOW);
    //Serial.print(tensione_radio5);
    //Serial.print(" ");
    /////////////////////
    // MEASUREMENT CURRENT ARDUINO RADIO 5V
    corrente_radio5 = analogRead(9);
    corrente_radio5=(Aref*corrente_radio5)/ValADC; // Digital data become in Voltage value
    corrente_radio5=-((corrente_radio5-offset_corrente)/m_corrente); // in mA
    //Serial.print(corrente_radio5);
    //Serial.print("     ");
    
    ///////TENSIONE RADIO 3.3V S5////// A=0100
    digitalWrite(33,LOW); 
    digitalWrite(35,LOW); 
    digitalWrite(37,HIGH); 
    digitalWrite(29,LOW); 
    digitalWrite(31,HIGH);
    delay(1);
    tensione_radio3 = analogRead(15);
    tensione_radio3=(Aref*tensione_radio3)/ValADC;
    digitalWrite(31,LOW);
    //Serial.print(tensione_radio3);
    //Serial.print(" ");
    /////////////////////
    // MEASUREMENT CURRENT ARDUINO RADIO 3.3V
    corrente_radio3 = analogRead(10);
    corrente_radio3=(Aref*corrente_radio3)/ValADC; // Digital data become in Voltage value
    corrente_radio3=-((corrente_radio3-offset_corrente)/m_corrente); // in mA
    //Serial.print(corrente_radio3);
    //Serial.print("     ");
    
    ///////TENSIONE TELECAMERE 5V S6////// A=0101
    digitalWrite(33,HIGH); 
    digitalWrite(35,LOW); 
    digitalWrite(37,HIGH); 
    digitalWrite(29,LOW); 
    digitalWrite(31,HIGH);
    delay(1);
    tensione_camera5[j] = analogRead(15);
    tensione_camera5[j]=(Aref*tensione_camera5[j])/ValADC;
    digitalWrite(31,LOW);
    tensione_camera5[10]=0;
    for(k=0;k<10;k++){
      tensione_camera5[10]=tensione_camera5[k]+tensione_camera5[10];
      }
    //Serial.print(tensione_camera5[10]/10);
    //Serial.print(" ");
    /////////////////////
    // MEASUREMENT CURRENT ARDUINO TELECAMERE 5V
    corrente_camera5[j] = analogRead(11);
    corrente_camera5[j]=(Aref*corrente_camera5[j])/ValADC; // Digital data become in Voltage value
    corrente_camera5[j]=-((corrente_camera5[j]-offset_corrente)/m_corrente); // in mA
    corrente_camera5[10]=0;
    for(k=0;k<10;k++){
      corrente_camera5[10]=corrente_camera5[k]+corrente_camera5[10];
      }
    //Serial.print(corrente_camera5[10]);
    //Serial.print("     ");
    
    ///////TENSIONE TELECAMERE 3.3V S7////// A=0110
    digitalWrite(33,LOW); 
    digitalWrite(35,HIGH); 
    digitalWrite(37,HIGH); 
    digitalWrite(29,LOW); 
    digitalWrite(31,HIGH);
    delay(1);
    tensione_camera3[j] = analogRead(15);
    tensione_camera3[j]=(Aref*tensione_camera3[j])/ValADC;
    digitalWrite(31,LOW);
    tensione_camera3[10]=0;
    for(k=0;k<10;k++){
      tensione_camera3[10]=tensione_camera3[k]+tensione_camera3[10];
      }
    //Serial.print(tensione_camera3[10]/10);
    //Serial.print(" ");
    /////////////////////
    // MEASUREMENT CURRENT ARDUINO TELECAMERE 3.3V
    corrente_camera3[j] = analogRead(12);
    corrente_camera3[j]=(Aref*corrente_camera3[j])/ValADC; // Digital data become in Voltage value
    corrente_camera3[j]=-((corrente_camera3[j]-offset_corrente)/m_corrente); // in mA
    corrente_camera3[10]=0;
    for(k=0;k<10;k++){
      corrente_camera3[10]=corrente_camera3[k]+corrente_camera3[10];
      }
    //Serial.print(corrente_camera3[10]/10);
    //Serial.print("        ");
    //////////////////////////////////////////////////////////////////////
    
    ///////TENSIONE SENSORI 5V S8////// A=0111
    digitalWrite(33,HIGH); 
    digitalWrite(35,HIGH); 
    digitalWrite(37,HIGH); 
    digitalWrite(29,LOW); 
    digitalWrite(31,HIGH);
    delay(1);
    tensione_sensor5[j] = analogRead(15);
    tensione_sensor5[j]=(Aref*tensione_sensor5[j])/ValADC;
    digitalWrite(31,LOW);
    tensione_sensor5[10]=0;
    for(k=0;k<10;k++){
      tensione_sensor5[10]=tensione_sensor5[k]+tensione_sensor5[10];
      }
    //Serial.print(tensione_sensor5[10]/10);
    //Serial.print(" ");
    ///////////////////////
    //// MEASUREMENT CURRENT ARDUINO SENSORI 5V
    corrente_sensor5[j] = analogRead(13);
    corrente_sensor5[j]=(Aref*corrente_sensor5[j])/ValADC; // Digital data become in Voltage value
    corrente_sensor5[j]=-((corrente_sensor5[j]-offset_corrente)/m_corrente); // in mA
    corrente_sensor5[10]=0;
    for(k=0;k<10;k++){
      corrente_sensor5[10]=corrente_sensor5[k]+corrente_sensor5[10];
      }
    //Serial.print(corrente_sensor5[10]/10);
    //Serial.print("     ");
    
    ///////TENSIONE SENSORI 3.3V S9////// A=1000
    digitalWrite(33,LOW); 
    digitalWrite(35,LOW); 
    digitalWrite(37,LOW); 
    digitalWrite(29,HIGH); 
    digitalWrite(31,HIGH);
    delay(1);
    tensione_sensor3[j] = analogRead(15);
    tensione_sensor3[j]=(Aref*tensione_sensor3[j])/ValADC;
    digitalWrite(31,LOW);
    tensione_sensor3[10]=0;
    for(k=0;k<10;k++){
      tensione_sensor3[10]=tensione_sensor3[k]+tensione_sensor3[10];
      }
    //Serial.print(tensione_sensor3[10]/10);
    //Serial.print(" ");
    ///////////////////////
    //// MEASUREMENT CURRENT ARDUINO SENSORI 3.3V
    corrente_sensor3[j] = analogRead(14);
    corrente_sensor3[j]=(Aref*corrente_sensor3[j])/ValADC; // Digital data become in Voltage value
    corrente_sensor3[j]=-((corrente_sensor3[j]-offset_corrente)/m_corrente); // in mA
    corrente_sensor3[10]=0;
    for(k=0;k<10;k++){
      corrente_sensor3[10]=corrente_sensor3[k]+corrente_sensor3[10];
      }
    //Serial.print(corrente_sensor3[10]/10);
    //Serial.print("     ");
    
    ///////Corrente Motori S10////// A=1001    occhio che la misura della potenza per questo e quello dopo non è corretta perchè non ho la miasura corretta della tensione anche
    digitalWrite(33,HIGH); 
    digitalWrite(35,LOW); 
    digitalWrite(37,LOW); 
    digitalWrite(29,HIGH); 
    digitalWrite(31,HIGH); //enable
    delay(1);
    corrente_elmo = analogRead(15);
    digitalWrite(31,LOW);
    corrente_elmo=(Aref*corrente_elmo)/ValADC;
    corrente_elmo=(corrente_elmo-Aref/2)/(0.1042*1);
    //Serial.print(corrente_elmo);                         //volendo si può aggiungere la corrente misurata dai drive
    //Serial.print("     ");
    //////////////////////////////////////////////////////////////////////
    
    ///////Corrente 10V S11////// A=1010
    digitalWrite(33,LOW); 
    digitalWrite(35,HIGH); 
    digitalWrite(37,LOW); 
    digitalWrite(29,HIGH); 
    digitalWrite(31,HIGH); //enable
    delay(1);
    corrente_10V = analogRead(15);
    digitalWrite(31,LOW);
    corrente_10V=(Aref*corrente_10V)/ValADC;
    corrente_10V=(corrente_10V-Aref/2)/(0.1042*3);
    //Serial.print(corrente_10V);                          //volendo si può aggiungere la corrente misurata dai sensori di corrente
    //Serial.print("     ");
    //////////////////////////////////////////////////////////////////////
    
    ///////Tensione 5V S12////// A=1011
    digitalWrite(33,HIGH); 
    digitalWrite(35,HIGH); 
    digitalWrite(37,LOW); 
    digitalWrite(29,HIGH); 
    digitalWrite(31,HIGH); //enable
    delay(1);
    tensione_5V = analogRead(15);
    tensione_5V=(Aref*tensione_5V)/ValADC;                //essendo il reference su questo stesso valore in caso di crollo dei 5v non avrebbe senso la lettura quindi mettere circuito che in assenza di questa porti la 3.3v sul reference con diodo
    digitalWrite(31,LOW);
    //Serial.print(tensione_5V);
    //Serial.print(" ");
    ///////Corrente 5V S13////// A=1100
    digitalWrite(33,LOW); 
    digitalWrite(35,LOW); 
    digitalWrite(37,HIGH); 
    digitalWrite(29,HIGH); 
    digitalWrite(31,HIGH); //enable                        //aggiungere su questa test che sia davvero 5v in caso alternativo si ha surriscaldamento del regolatore e vanno spente tutte le schede. aggiungere inoltre gestione on off regolatore da pin digitale
    delay(1);
    corrente_5V = analogRead(15);
    digitalWrite(31,LOW);
    corrente_5V=(Aref*corrente_5V)/ValADC;
    corrente_5V=(corrente_5V-Aref/2)/(0.1042*3);
    //Serial.print(corrente_5V);                          //volendo si può aggiungere la differenza tra la somma di tutte e questo totale
    //Serial.print("     ");
    //////////////////////////////////////////////////////////////////////
    
    
    
    ///////Tensione batteria 12V S14////// A=1101
    digitalWrite(33,HIGH); 
    digitalWrite(35,LOW); 
    digitalWrite(37,HIGH); 
    digitalWrite(29,HIGH); 
    digitalWrite(31,HIGH); //enable
    delay(1);
    tensione_12V = analogRead(15);
    digitalWrite(31,LOW);
    tensione_12V=(Aref*tensione_12V)/ValADC*8.0565957-2.0425239;            //fare partitore preciso /3 con zener protezione a 5V
    //Serial.println(tensione_12V);                          
    //////////////////////////////////////////////////////////////////////
    }
  
  
  
  
  ////////////////////////MISURA DELLA POTENZA TOTALE ISTANTANEA//////////
  total_power=(tensione_master[10]*corrente_master[10])+(tensione_power[10]*corrente_power[10])+(tensione_motori[10]*corrente_motori[10])+(tensione_sensor5[10]*corrente_sensor5[10])+(tensione_sensor3[10]*corrente_sensor3[10])+(9.8*corrente_10V);
  //Serial.print("TOTAL POWER ");
  //Serial.print(total_power);
  //Serial.println(" mW");
  //Serial.println(" ");
  //////////////////////////////////////////////////////////////////////////
  
  getInterrupt();
            
  digitalWrite(13, LOW);
  }
  
if (millis()-WDT>=10000) //il master è morto
  {
  sw_nodo(false,0);
  delay(500);
  sw_nodo(true,0);
  WDT=millis();  
  }
  
} //end loop


void getInterrupt()
{
   if (tensione_5V<4.3)
      {
      digitalWrite(12,LOW);
      delay(100);
      digitalWrite(12,HIGH);
      }
   if (tensione_12V<10.5)
      {
      digitalWrite(12,LOW);
      delay(100);
      digitalWrite(12,HIGH);
      }
   if (corrente_master[10]/10>300)
      {
      digitalWrite(12,LOW);
      delay(100);
      digitalWrite(12,HIGH);
      }
   if (corrente_power[10]/10>300)
      {
      digitalWrite(12,LOW);
      delay(100);
      digitalWrite(12,HIGH);
      }
   if (corrente_motori[10]/10>300)
      {
      digitalWrite(12,LOW);
      delay(100);
      digitalWrite(12,HIGH);
      }
//   if (corrente_radio5>300)
//      {
//      digitalWrite(12,LOW);
//      delay(100);
//      digitalWrite(12,HIGH);
//      }
//   if (corrente_camera5[10]/10>300)
//      {
//      digitalWrite(12,LOW);
//      delay(100);
//      digitalWrite(12,HIGH);
//      }
   if (corrente_sensor5[10]/10>300)
      {
      digitalWrite(12,LOW);
      delay(100);
      digitalWrite(12,HIGH);
      }
   if (corrente_5V>1)
      {
      digitalWrite(12,LOW);
      delay(100);
      digitalWrite(12,HIGH);
      }
   if (corrente_elmo>3)
      {
      digitalWrite(12,LOW);
      delay(100);
      digitalWrite(12,HIGH);
      }
  }


//routine spegnimento, accensione nodo
void sw_nodo(boolean sw,int scheda) //da 0 a 6 master,potenza,motori,radio,camera,sensori
  {
    if (scheda==6)
      {
      if (sw)
        digitalWrite(40,HIGH);
      else
        digitalWrite(40,LOW);
      }
    else if (sw)
      {
        stato[scheda][0]=true;
        stato[scheda][1]=true;
        digitalWrite(42+2*scheda,HIGH);
        digitalWrite(42+2*scheda+1,LOW);
      }
    else
      {
        stato[scheda][1]=false;
        digitalWrite(42+2*scheda,LOW);
        digitalWrite(42+2*scheda+1,LOW);
      }
  }
  
  
void receiveEvent(int num)
{
  i2c=Wire.read();
  
  //0=MASTER  1=POTENZA   2=MOTORI   3=RADIO    4=TELECAMERE    5=SENSORI   6=MOTORI12v
  if (i2c==10)
    sw_nodo(true,0);
  else if (i2c==11)
    sw_nodo(true,1);
  else if (i2c==12)
    sw_nodo(true,2);
  else if (i2c==13)
    sw_nodo(true,3);
  else if (i2c==14)
    sw_nodo(true,4);
  else if (i2c==15)
    sw_nodo(true,5);
  else if (i2c==16)
    sw_nodo(true,6);
    
  else if (i2c==20)
    {
    sw_nodo(false,0);
    delay(500);
    sw_nodo(true,0);
    }
  else if (i2c==21)
    sw_nodo(false,1);
  else if (i2c==22)
    sw_nodo(false,2);
  else if (i2c==23)
    sw_nodo(false,3);
  else if (i2c==24)
    sw_nodo(false,4);
  else if (i2c==25)
    sw_nodo(false,5);
  else if (i2c==26)
    sw_nodo(false,6);
    
  else if ((i2c>=30)&&(i2c<=35))
    {
    stato[i2c-30][0]=true;
    digitalWrite(42+2*(i2c-30),HIGH);
    delay(10);  //ritardo 10 ms per non creare buchi nell'alimentazione
    digitalWrite(43+2*(i2c-30),LOW);
    }
  
  else if ((i2c>=40)&&(i2c<=45))
    {
    stato[i2c-40][0]=false;
    digitalWrite(43+2*(i2c-40),HIGH);
    //delay(10);
    digitalWrite(42+2*(i2c-40),LOW);
    }
 
  else if (i2c==50)
    Tc=1*1000;
  else if (i2c==51)
    Tc=2*1000;
  else if (i2c==52)
    Tc=5*1000;
  else if (i2c==53)
    Tc=10*1000;
  else if (i2c==54)
    Tc=30*1000;
  else if (i2c==55)
    Tc=60*1000; 
    
  else if (i2c==60)
    digitalWrite(41,LOW);
  else if (i2c==61)
    digitalWrite(41,HIGH);
  
  
}
  
void requestEvent()
{
  int dati[16]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
  byte i;
  WDT=millis();
  
  switch (i2c)
  {
    case 0:
      for (i=0;i<6;i++)
      {
         dati[i]=int(BatteryPercent[i]);
         if (stato[i][0])
            bitSet(dati[i],15);//ricarica
         if (stato[i][1])
            bitSet(dati[i],14);//accesa
      }
      dati[6]=int(tensione_master[10]/10*100);
      dati[7]=int(corrente_master[10]/10);
      dati[8]=int(tensione_power[10]/10*100);
      dati[9]=int(corrente_power[10]/10);
      dati[10]=int(tensione_motori[10]/10*100);
      dati[11]=int(corrente_motori[10]/10);
      dati[12]=0;//int(tensione_radio5*100);
      dati[13]=0;//int(corrente_radio5);
      dati[14]=0;//int(tensione_radio3*100);
      dati[15]=0;//int(corrente_radio3);
    break;
    case 1:
      dati[0]=0;//int(tensione_camera5[10]/10*100);
      dati[1]=0;//int(corrente_camera5[10]/10);
      dati[2]=0;//int(tensione_camera3[10]/10*100);
      dati[3]=0;//int(corrente_camera3[10]/10);
      dati[4]=int(tensione_sensor5[10]/10*100);
      dati[5]=int(corrente_sensor5[10]/10);
      dati[6]=int(tensione_sensor3[10]/10*100);
      dati[7]=int(corrente_sensor3[10]/10);
      dati[8]=0;//int(corrente_elmo*1000);
      dati[9]=int(corrente_10V*1000);
      dati[10]=int(tensione_5V*100);
      dati[11]=0;//int(corrente_5V*1000);
      dati[12]=int(tensione_12V*100);
      dati[13]=int(total_power);
      if (tensione_master[10]/10<4.5)
        bitSet(dati[14],15);
      if (tensione_power[10]/10<4.5)
        bitSet(dati[14],14);
      if (tensione_motori[10]/10<4.5)
        bitSet(dati[14],13);
//      if (tensione_radio5<4.5)
//        bitSet(dati[14],12);
//      if (tensione_camera5[10]/10<4.5)
//        bitSet(dati[14],11);
//      if (tensione_sensor5[10]/10<4.5)
//        bitSet(dati[14],10);
      if (tensione_5V<4.5)
        bitSet(dati[14],9);
      if (tensione_12V<21)
        bitSet(dati[14],8);
      if (corrente_master[10]/10>500)
        bitSet(dati[14],7);
      if (corrente_power[10]/10>300)
        bitSet(dati[14],6);
      if (corrente_motori[10]/10>500)
        bitSet(dati[14],5);
//      if (corrente_radio5>300)
//        bitSet(dati[14],4);
//      if (corrente_camera5[10]/10>300)
//        bitSet(dati[14],3);
      if (corrente_sensor5[10]/10>300)
        bitSet(dati[14],2);
//      if (corrente_5V>1)
//        bitSet(dati[14],1);
//      if (corrente_elmo>3)
//        bitSet(dati[14],0);  
      dati[15]=int((millis()-time_count)/1000);
    break;
  }
  
  invia_i2c(dati,NUM_DATI);
}


void integerToBytes(int val, byte b[2]) {
  b[0] = (byte )((val >> 8) & 0xff);
  b[1] = (byte )(val & 0xff);
}

void invia_i2c (int *dato, byte num_dati)
{
  byte i=0;
  byte one[num_dati*2];
  for (i=0;i<num_dati; i++)
  {
    integerToBytes(dato[i], &one[i*2]);
    
  }
   Wire.write(one,num_dati*2);
}
